#include <functional>
#include <gazebo/gazebo.hh>
#include <string>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>


namespace gazebo
{
    class FakeRotorPlugin : public ModelPlugin
    {
    public:
        void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
        {
            // Store the pointer to the model
            this->_model = _parent;
            std::string joint_name_;

            if (_sdf->HasElement("jointSpeed"))
            {
                this->_speed = _sdf->GetElement("jointSpeed")->Get<double>();
            }
            else gzerr << "[FakeRotorPlugin] Please specify a speed, where the rotor "
                          "is attached.\n";

            if (_sdf->HasElement("jointName"))
            {
                joint_name_ = _sdf->GetElement("jointName")->Get<std::string>();
                this->_joint = this->_model->GetJoint(joint_name_);
            }
            else gzerr << "[FakeRotorPlugin] Please specify a jointName, where the rotor "
                          "is attached.\n";

            if (_sdf->HasElement("turningDirection"))
            {
                std::string turning_direction =
                    _sdf->GetElement("turningDirection")->Get<std::string>();
                if (turning_direction == "cw")
                    this->_turning_direction = -1;
                else if (turning_direction == "ccw")
                    this->_turning_direction = 1;
                else
                    gzerr << "[gazebo_motor_model] Please only use 'cw' or 'ccw' as "
                             "turningDirection.\n";
            }
            else
                gzerr << "[gazebo_motor_model] Please specify a turning direction ('cw' or "
                         "'ccw').\n";

            // Listen to the update event. This event is broadcast every
            // simulation iteration.
            this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                std::bind(&FakeRotorPlugin::OnUpdate, this));
        }

        // Called by the world update start event
    public:
        void OnUpdate()
        {
            // Apply a small linear velocity to the model.
            this->_joint->SetVelocity(
                0, this->_speed * this->_turning_direction);
        }

        // Pointer to the model
    private:
        physics::ModelPtr _model;
        physics::JointPtr _joint;
        double _speed;
        int _turning_direction;

        // Pointer to the update event connection
    private:
        event::ConnectionPtr updateConnection;
    };

    // Register this plugin with the simulator
    GZ_REGISTER_MODEL_PLUGIN(FakeRotorPlugin)
}